#include <cmath>
#include <iostream>
#include <algorithm>
#include "astar.h"

using namespace std;
using namespace LVGame;

//=============================================================================

int AStar::HeursiticCostEstimate::operator()(const Node *lhs, const Node *rhs)
{
    int dx = rhs->pos.x - lhs->pos.x;
    int dy = rhs->pos.y - lhs->pos.y;

    return static_cast<int>(sqrt(dx * dx + dy * dy) * 10);
}

bool AStar::NodeFScoreLower::operator()(const Node *lhs, const Node *rhs)
{
    return lhs->f < rhs->f;
}

int AStar::NodeNeighborGCost::operator()(const Node *lhs, const Node *rhs)
{
    int dx = rhs->pos.x - lhs->pos.x;
    int dy = rhs->pos.y - lhs->pos.y;

    if (0 == dx || 0 == dy) {
        return 10;
    } else {
        return 14;
    }
}

//=============================================================================

void AStar::operator()(const int *search_map, int x_max, int y_max,
                       const Point &start_pos, const Point &end_pos,
                       std::vector<Point> *result_path)
{
    init(search_map, x_max, y_max, start_pos, end_pos);

    m_start_node->g = 0;
    m_start_node->f = m_start_node->g + HeursiticCostEstimate()(m_start_node, m_end_node);
    insertOpenList(m_start_node);

    vector<Node *> neighbor_nodes;
    neighbor_nodes.reserve(8);

    while (!isOpenListEmpty()) {
        Node *cur_node = getMinFScoreNodeInOpenList();
        if (cur_node == m_end_node) {
            constructResultPath(result_path);
            return;
        }

        deleteMinFScoreNodeInOpenList();
        insertCloseList(cur_node);

        getNeighborNodes(cur_node, &neighbor_nodes);

        for (size_t i = 0; i < neighbor_nodes.size(); ++i) {
            Node *neighbor = neighbor_nodes[i];

            bool is_in_open_list = isInOpenList(neighbor);
            bool is_in_close_list = isInCloseList(neighbor);
            int g_cal = cur_node->g + NodeNeighborGCost()(cur_node, neighbor);

            if (is_in_close_list && g_cal >= neighbor->g) {
                continue;
            }

            if (!is_in_open_list || g_cal < neighbor->g) {
                neighbor->parent = cur_node;
                neighbor->g = g_cal;
                neighbor->f = neighbor->g + HeursiticCostEstimate()(neighbor, m_end_node);
                if (!is_in_open_list) {
                    insertOpenList(neighbor);
                } else {
                    resortOpenList(neighbor);
                }
            }
        }
    }
}

void AStar::init(const int *search_map, int x_max, int y_max,
                 const Point &start_pos, const Point &end_pos)
{
    m_x_max = x_max;
    m_y_max = y_max;

    for (int y = 0; y < m_y_max; ++y) {
        for (int x = 0; x < m_x_max; ++x) {
            Node node;
            node.pos = Point(x, y);
            node.is_walkable = !search_map[y * m_x_max + x];
            node.f = 0;
            node.g = 0;
            node.parent = NULL;
            node.open_list_pos = 0;
            node.close_list_pos = 0;
            m_grid.push_back(node);
        }
    }

    m_start_node = getNode(start_pos);
    initOpenList();
    m_end_node = getNode(end_pos);
}

void AStar::constructResultPath(std::vector<Point> *result_path)
{
    Node *node = m_end_node;

    while (node != m_start_node) {
        result_path->push_back(node->pos);
        node = node->parent;
    }
    std::reverse(result_path->begin(), result_path->end());
}

AStar::Node *AStar::getNode(const Point &pos)
{
    return &m_grid[pos.y * m_x_max + pos.x];
}

void AStar::getNeighborNodes(const Node *node, std::vector<Node *> *neighbor_nodes)
{
    neighbor_nodes->clear();

    int start_x = std::max(node->pos.x - 1, 0);
    int end_x = std::min(node->pos.x + 1, m_x_max - 1);
    int start_y = std::max(node->pos.y - 1, 0);
    int end_y = std::min(node->pos.y + 1, m_y_max - 1);

    for (int y = start_y; y <= end_y; ++y) {
        for (int x = start_x; x <= end_x; ++x) {
            if (!(x == node->pos.x && y == node->pos.y)) {
                Node* neighbor = getNode(Point(x, y));

                if (neighbor->is_walkable) {
                    neighbor_nodes->push_back(neighbor);
                }
            }
        }
    }
}

void AStar::initOpenList()
{
    m_open_list.clear();
    m_open_list.push_back(NULL);
}

bool AStar::isInOpenList(const Node *node) const
{
    return node->open_list_pos;
}

bool AStar::isOpenListEmpty() const
{
    return m_open_list.size() <= 1;
}

void AStar::insertOpenList(Node* node)
{
    size_t cur_index = m_open_list.size();
    m_open_list.push_back(node);
    node->open_list_pos = cur_index;

    for (;;) {
        size_t parent_index = cur_index / 2;

        if (0 == parent_index) {
            break;
        }
        if (NodeFScoreLower()(m_open_list[parent_index], m_open_list[cur_index])) {
            break;
        }

        m_open_list[parent_index]->open_list_pos = cur_index;
        m_open_list[cur_index]->open_list_pos = parent_index;
        std::swap(m_open_list[parent_index], m_open_list[cur_index]);

        cur_index = parent_index;
    }
}

AStar::Node *AStar::getMinFScoreNodeInOpenList()
{
    if (isOpenListEmpty()) {
        return NULL;
    } else {
        return m_open_list[1];
    }
}

void AStar::deleteMinFScoreNodeInOpenList()
{
    if (isOpenListEmpty()) {
        return;
    }

    m_open_list[1]->open_list_pos = 0;

    m_open_list[1] = m_open_list.back();
    m_open_list.pop_back();

    size_t cur_index = 1;
    for (;;) {
        size_t child_index = cur_index * 2;

        if (child_index >= m_open_list.size()) {
            break;
        }
        if (child_index + 1 < m_open_list.size() &&
            NodeFScoreLower()(m_open_list[child_index + 1], m_open_list[child_index])) {
            ++child_index;
        }

        if (NodeFScoreLower()(m_open_list[cur_index], m_open_list[child_index])) {
            break;
        }

        m_open_list[cur_index]->open_list_pos = child_index;
        m_open_list[child_index]->open_list_pos = cur_index;
        std::swap(m_open_list[cur_index], m_open_list[child_index]);

        cur_index = child_index;
    }
}

void AStar::resortOpenList(Node* node)
{
    size_t cur_index = node->open_list_pos;

    for (;;) {
        size_t parent_index = cur_index / 2;

        if (0 == parent_index) {
            break;
        }
        if (NodeFScoreLower()(m_open_list[parent_index], m_open_list[cur_index])) {
            break;
        }

        m_open_list[parent_index]->open_list_pos = cur_index;
        m_open_list[cur_index]->open_list_pos = parent_index;
        std::swap(m_open_list[parent_index], m_open_list[cur_index]);

        cur_index = parent_index;
    }
}

bool AStar::isInCloseList(const Node *node) const
{
    return node->close_list_pos;
}

void AStar::insertCloseList(Node *node)
{
    node->close_list_pos = 1;
}

